#include "PhysxPicking.h"

#include "Game.h"

#include "WindowsCompatibility/Physx.h"
#include "WindowsCompatibility/OpenGL.h"
#include <NxRay.h>
#include <NxShape.h>
#include <NxSphereShapeDesc.h>
#include <NxDistanceJointDesc.h>

NxActor* PhysxPicking::grabbedActor = NULL;
NxActor* PhysxPicking::mouseCursorActor = NULL;
NxDistanceJoint* PhysxPicking::mouseCursorJoint = NULL;
NxReal PhysxPicking::mouseDepth = 0.0f;

bool PhysxPicking::WindowToObjectCoords(const int wx, const int wy, const float wz, NxVec3& objectCoords) {
	GLint viewport[4];
	GLdouble modelviewMatrix[16];
	GLdouble projectionMatrix[16];
	glGetIntegerv(GL_VIEWPORT, viewport);
	glGetDoublev(GL_MODELVIEW_MATRIX, modelviewMatrix);
	glGetDoublev(GL_PROJECTION_MATRIX, projectionMatrix);
	GLdouble winx, winy, winz;
	GLint ret = gluUnProject((GLdouble)wx, (GLdouble)(viewport[3] - wy), (GLdouble)wz, modelviewMatrix, projectionMatrix, viewport, &winx, &winy, &winz);
	objectCoords.set((NxReal)winx, (NxReal)winy, (NxReal)winz);
	return (ret != 0); //true -> no error
}

bool PhysxPicking::ObjectToWindowCoords(const NxVec3& objectCoords, int& wx, int& wy, float& wz) {
	GLint viewport[4];
	GLdouble modelviewMatrix[16];
	GLdouble projectionMatrix[16];
	glGetIntegerv(GL_VIEWPORT, viewport);
	glGetDoublev(GL_MODELVIEW_MATRIX, modelviewMatrix);
	glGetDoublev(GL_PROJECTION_MATRIX, projectionMatrix);
	GLdouble winx, winy, winz;
	GLint ret = gluProject((GLdouble)objectCoords.x, (GLdouble)objectCoords.y, (GLdouble)objectCoords.z, modelviewMatrix, projectionMatrix, viewport, &winx, &winy, &winz);
	wx = (int)winx;
	wy = viewport[3] - (int)winy - 1;
	wz = (float)winz;
	return (ret != 0); //true -> no error
}

NxVec3 PhysxPicking::GroundIntersection(const int wx, const int wy) {
	NxRay ray;
	bool noError = true;
	noError &= WindowToObjectCoords(wx, wy, 0.0f, ray.orig);
	noError &= WindowToObjectCoords(wx, wy, 1.0f, ray.dir);
	if(noError == false) {
		Game::log.LogError("PhysxPicking::GroundIntersection: WindowToObjectCoords calculation failed.");
		return NULL; //error occurred
	}
	ray.dir = ray.dir - ray.orig;
	
	const float s = - ray.orig.y / ray.dir.y;
	return ray.orig + ray.dir*s;
}

NxActor* PhysxPicking::SelectActor(const int wx, const int wy, const NxScene* scene) {
	NxRay ray;
	bool noError = true;
	noError &= WindowToObjectCoords(wx, wy, 0.0f, ray.orig);
	noError &= WindowToObjectCoords(wx, wy, 1.0f, ray.dir);
	if(noError == false) {
		Game::log.LogError("PhysxPicking::SelectActor: WindowToObjectCoords calculation failed.");
		return NULL; //error occurred
	}
	ray.dir = ray.dir - ray.orig;
	ray.dir.normalize();

	NxRaycastHit hit;
	NxShape* closestShape = scene->raycastClosestShape(ray, NX_ALL_SHAPES, hit);
	if(closestShape != NULL) {
		return &(closestShape->getActor());
	} else {
		return NULL;
	}
}

bool PhysxPicking::GrabActor(const int wx, const int wy, NxScene* scene) {
	ReleaseGrabbedActor(scene);

	//Determine clicked actor to grab
	NxRay ray;
	bool noError = true;
	noError &= WindowToObjectCoords(wx, wy, 0.0f, ray.orig);
	noError &= WindowToObjectCoords(wx, wy, 1.0f, ray.dir);
	if(noError == false) {
		Game::log.LogError("PhysxPicking::GrabActor: WindowToObjectCoords calculation failed.");
		return false; //error occurred
	}
	ray.dir = ray.dir - ray.orig;
	ray.dir.normalize();

	NxRaycastHit hit;
	NxShape* closestShape = scene->raycastClosestShape(ray, NX_ALL_SHAPES, hit);
	if(closestShape != NULL) {
		grabbedActor = &(closestShape->getActor());
	} else {
		return false;
	}

	//getMouseDepth
	int hitWx, hitWy;
	noError = true;
	noError &= ObjectToWindowCoords(hit.worldImpact, hitWx, hitWy, mouseDepth); 
	if(noError == false) {
		Game::log.LogError("PhysxPicking::GrabActor: ObjectToWindowCoords calculation failed.");
		return false; //error occurred
	}

	// Create mouseCursorActor: sphere. For attaching the joint
	NxSphereShapeDesc sphereDesc;
	sphereDesc.radius = 0.1f;

	NxBodyDesc bodySphereDesc;
	NxActorDesc actorSphereDesc;	
	actorSphereDesc.shapes.pushBack(&sphereDesc);	
	actorSphereDesc.body = &bodySphereDesc;
	actorSphereDesc.density = 1.0f;
	actorSphereDesc.globalPose.t = hit.worldImpact;
	mouseCursorActor = scene->createActor(actorSphereDesc);

	mouseCursorActor->raiseBodyFlag(NX_BF_KINEMATIC);
	mouseCursorActor->raiseActorFlag(NX_AF_DISABLE_COLLISION);
	//	

	grabbedActor->wakeUp();

	//Create distancejoint	
	NxDistanceJointDesc dJointDesc;
	dJointDesc.actor[0] = mouseCursorActor;
	dJointDesc.actor[1] = grabbedActor;
	mouseCursorActor->getGlobalPose().multiplyByInverseRT(hit.worldImpact, dJointDesc.localAnchor[0]);
	grabbedActor->getGlobalPose().multiplyByInverseRT(hit.worldImpact, dJointDesc.localAnchor[1]);
	dJointDesc.maxDistance = dJointDesc.minDistance = 0.0f;
	dJointDesc.spring.damper = 1.0f;
	dJointDesc.spring.spring = 200.0f;
	dJointDesc.flags |= NX_DJF_MAX_DISTANCE_ENABLED | NX_DJF_SPRING_ENABLED;
	mouseCursorJoint = (NxDistanceJoint*)scene->createJoint(dJointDesc);

	return true;
}

bool PhysxPicking::MoveGrabbedActor(const int wx, const int wy) {
	if(mouseCursorActor == NULL) {
		return false;
	}
	NxVec3 pos;
	bool noError = true;
	noError &= WindowToObjectCoords(wx, wy, mouseDepth, pos);
	if(noError == false) {
		Game::log.LogError("PhysxPicking::MoveGrabbedActor: WindowToObjectCoords calculation failed.");
		return false; //error occurred
	}
	mouseCursorActor->setGlobalPosition(pos);
	grabbedActor->wakeUp();
	return true;
}

void PhysxPicking::ReleaseGrabbedActor(NxScene* scene) {
	if(mouseCursorJoint != NULL) {
		scene->releaseJoint(*mouseCursorJoint);
	}
	mouseCursorJoint = NULL;
	if(mouseCursorActor != NULL) {
		scene->releaseActor(*mouseCursorActor);
	}
	mouseCursorActor = NULL;
	grabbedActor = NULL;
}
